#! /opt/local/bin/python2.6
import numpy as np
import time
import sys
from rwEXR import *


# 3744 23.9
# 5616 35.8


# distortion correction parameters in rad
# 0.207029529537 0.0422547753997


entrancePixel = (3473,5615-3043)

#entrancePixel = (1848,5615-2341)
#center = (1867.5,5616-2979.5)
#center = (1884.18,2834.89)
#center = np.array([2836.5,3744-1860.79])

center = (1853,5616-2874.5)
center = (1860.79,5615-2836.5)
#center = np.array([2836.5,3744-1860.79])


def Rd(pixel):
    c = center
    return np.sqrt( (pixel[0] - c[0])*(pixel[0] - c[0]) + (pixel[1] - c[1])*(pixel[1] - c[1]) )


pix = 0.00637738462

coeffs = [ -1.54035227e-12 ,  6.94350405e-09 , -1.22082544e-05  , 9.47349416e-03,          5.90440253e+00]

poly_Rd2f = np.poly1d(coeffs)

f = poly_Rd2f(Rd(entrancePixel))

print f


d = 79+89

T1 = np.matrix([d,-d,0])

R = np.matrix([[0,-1,0],[1,0,0],[0,0,1]])

e1 = np.matrix([-f,d+f,0]) 

e2 = np.matrix([d+f,-f,0])

theta = 2*np.arcsin( Rd(entrancePixel)  / (2.0 * f/pix) )

phi   = np.arctan2( entrancePixel[1] - center[1], entrancePixel[0] - center[0] )

op1 = np.matrix([ np.sin(theta)*np.cos(phi), np.cos(theta), np.sin(theta)*np.sin(phi)])

#E = np.matrix( [[0,0,-d],[0,0,-d],[d,-d,0]])

N = np.cross(op1,T1) 

print N

#N = (op1 * E)

N = N.flat

n1 = N[0]
n2 = N[1]
n3 = N[2]

n4 = -n1*(d+f) + n2*f
    
y = np.arange(-1000,1000,0.1)

z = (-n2*f-n2*y)/(n3)

ru = np.sqrt(z*z+y*y)


outPixel = (1785,5615-3001)
f = poly_Rd2f(Rd(outPixel))
print f

rd = 2*f*np.sin(np.arctan2(ru,f)/2)

U = -rd * np.cos(np.arctan2(z,y)) * pix
V = rd * np.sin(np.arctan2(z,y)) * pix

exrR,exrG,exrB,L,size = readExr("/Network/scratch/Tests/XL/DepthEstimationProject/sourceimages/HDRI/rotationFisheye/exr/vue1.exr")

for (u,v) in zip(U,V):
    
    L[u+1860.79,v+(5615-2836.5)] = 255


createNewOutputImage("epipolar.exr",L.T,L.T,L.T,size)

print e1 * E 










